﻿#include <vector>
#include <tuple>
#include <algorithm>
#include <unordered_set>
#include <assert.h>
namespace as
{
namespace tools
{
	template<class Point>
	class Node
	{
	public:
		Node *parent = nullptr;
		Node *child = nullptr;
		//起点到当前点代价
		float g = 0.0f;
		//当前点到终点代价
		float h = 0.0f;
		float f = 0.0f;
		Point pos;
	};

	template<class Point, class Alloc = std::allocator<Node<Point> > >
	class AStarSearch
	{
	public:
		AStarSearch()
		{

		}
		~AStarSearch()
		{
			FreeNoUseNode("del obj");
		}

		enum State : unsigned char
		{
			SEARCH_STATE_NON,
			SEARCH_STATE_SUCCESS,
			SEARCH_STATE_OPEN_LIST_EMPTY,
			SEARCH_STATE_NOT_PATH,
		};
		typedef Node<Point> NodePoint;
		typedef std::tuple<State, std::vector<NodePoint *> > Result;
	
		class HeapComact
		{
		public:
			bool operator()(const NodePoint *l, const NodePoint *r)
			{
				return l->f > r->f;
			}
		};

		void FreeUseNode()
		{
			if (start_pos_)
			{
				NodePoint *node = start_pos_;
				if (node->child)
				{
					do
					{
						NodePoint *child = node->child;
						release(node);
						node = child;
					} while (node);
				}
				else
				{
					release(start_pos_);
					release(end_pos_);
				}
				start_pos_ = nullptr;
			}

			end_pos_ = nullptr;
		}

		void FreeAllNode(const char *log = "defalut")
		{
			for (auto node : open_list_)
			{
				release(node);
			}
			
			for (auto node : close_list_)
			{
				release(node);
			}

			open_list_.clear();
			close_list_.clear();
			success_list_.clear();
			start_pos_ = nullptr;

			release(end_pos_);
			end_pos_ = nullptr;
			assert(alloc_count_ == 0);
		}

		void FreeNoUseNode(const char *log = "defalut")
		{
			for (auto node : open_list_)
			{
				if(node->child == nullptr)
					release(node);
			}

			for (auto node : close_list_)
			{
				if(node->child == nullptr)
					release(node);
			}

			open_list_.clear();
			close_list_.clear();
			std::cout << log << "-alloc count:" << alloc_count_ << "\n";
		}

		void SetStartAndEndPos(const Point &start, const Point &end)
		{
			state_ = SEARCH_STATE_NON;
			int checkCount = alloc_count_;
			if (start_pos_ == nullptr)
			{
				start_pos_ = alloc(start);
			}
			else
			{
				start_pos_->pos = start;
				checkCount--;
			}

			if (end_pos_ == nullptr)
			{
				end_pos_ = alloc(end);
			}
			else
			{
				end_pos_->pos = end;
				checkCount--;
			}

			assert(checkCount == 0);

			end_pos_->child = nullptr;
			end_pos_->parent = nullptr;
			end_pos_->g = 0;
			end_pos_->h = 0;
			end_pos_->f = 0;
		
			FreeNoUseNode("start");
			success_list_.clear();

			start_pos_->g = 0;
			start_pos_->h = start.DistanceMetric(end);
			start_pos_->f = start_pos_->g + start_pos_->h;
			start_pos_->parent = nullptr;
			start_pos_->child = nullptr;
			step_ = 0;

			open_list_.push_back(start_pos_);
			push_heap(open_list_.begin(), open_list_.end(), HeapComact());
		}

		State GetSuccessPath()
		{
			if (state_ == SEARCH_STATE_SUCCESS)
				return state_;
			if (state_ != SEARCH_STATE_NON)
				return state_;

			if (open_list_.empty())
			{
				state_ = SEARCH_STATE_OPEN_LIST_EMPTY;
				return state_;
			}
			
			while (open_list_.size() > 0)
			{
				NodePoint *n = open_list_.front();
				pop_heap(open_list_.begin(), open_list_.end());
				open_list_.pop_back();
				step_++;
				if (n->pos.IsSame(end_pos_->pos))
				{
					end_pos_->parent = n->parent;
					end_pos_->g = n->g;
					if (n->pos.IsSame(start_pos_->pos) == false)
					{
						release(n);
						NodePoint *nodeChild = end_pos_;
						NodePoint *nodeParent = end_pos_->parent;
						do
						{
							nodeParent->child = nodeChild;

							nodeChild = nodeParent;
							nodeParent = nodeParent->parent;

						} while (nodeChild != start_pos_);
					}

					FreeNoUseNode("done");
					state_ = SEARCH_STATE_SUCCESS;
					return state_;
				}

				success_list_.clear();
				bool ret = n->pos.GetSuccessPos(this, n->parent ? &n->parent->pos : nullptr);
				//没有下一个点
				if (ret == false)
				{
					for (auto node : success_list_)
					{
						release(node);
						node = nullptr;
					}
					success_list_.clear();

					release(n);
					FreeAllNode("not find pos");
					state_ = SEARCH_STATE_NOT_PATH;
					return state_;
				}

				for (auto successNode : success_list_)
				{
					float newg = n->g + n->pos.GetCost(successNode->pos);
					auto checkSamePos = [successNode](const NodePoint *node)
						{
							return node->pos.IsSame(successNode->pos);
						};

					auto openIt = std::find_if(open_list_.begin(), open_list_.end(), checkSamePos);
					if (openIt != open_list_.end())
					{
						if ((*openIt)->g <= newg)
						{
							release(successNode);
							continue;
						}
					}

					auto closeIt = std::find_if(close_list_.begin(), close_list_.end(), checkSamePos);
					if (closeIt != close_list_.end())
					{
						if ((*closeIt)->g <= newg)
						{
							release(successNode);
							continue;
						}
					}

					successNode->parent = n;
					successNode->g = newg;
					successNode->h = successNode->pos.DistanceMetric(end_pos_->pos);
					successNode->f = successNode->g + successNode->h;

					if (closeIt != close_list_.end())
					{
						(*closeIt)->parent = successNode->parent;
						(*closeIt)->g = successNode->g;
						(*closeIt)->h = successNode->h;
						(*closeIt)->f = successNode->f;
						release(successNode);
						open_list_.push_back(*closeIt);
						close_list_.erase(closeIt);
						push_heap(open_list_.begin(), open_list_.end(), HeapComact());
					}
					else if (openIt != open_list_.end())
					{
						(*openIt)->parent = successNode->parent;
						(*openIt)->g = successNode->g;
						(*openIt)->h = successNode->h;
						(*openIt)->f = successNode->f;
						release(successNode);
						make_heap(open_list_.begin(), open_list_.end(), HeapComact());
					}
					else
					{
						open_list_.push_back(successNode);
						push_heap(open_list_.begin(), open_list_.end(), HeapComact());
					}
				}
				close_list_.insert(n);
			}
		
			if (open_list_.empty())
			{
				FreeAllNode("path empty");
				state_ = SEARCH_STATE_OPEN_LIST_EMPTY;
				return state_;
			}
			return state_;
		}

		void AddSuccessPos(const Point &pos)
		{
			NodePoint *node = alloc(pos);
			success_list_.push_back(node);
		}

		NodePoint *GetStartNode()
		{
			return start_pos_;
		}

		NodePoint *GetEndNode()
		{
			return end_pos_;
		}

		void PrintPath()
		{
			std::vector<NodePoint *> vec;
			NodePoint *curr = end_pos_;
			while (curr && curr != start_pos_)
			{
				vec.push_back(curr);
				curr = curr->parent;
			}
			reverse(vec.begin(), vec.end());
			std::cout << "start print\n";
			int i = 0;
			for (auto node : vec)
			{
				std::cout << node->pos.x_ << "-" << node->pos.y_ << " ";
				i++;
				if (i == 10)
				{
					i = 0;
					std::cout << "\n";
				}
			}
			std::cout << "end print\n";
		}

	protected:
		NodePoint *alloc(const Point &pos)
		{
			auto node = alloc();
			//std::cout << "alloc node:" << node << " pos:" << pos.x_ << "-" << pos.y_ << "\n";
			node->pos = pos;
			return node;
		}

		NodePoint *alloc()
		{
			NodePoint *node = nullptr;
			node = alloc_.allocate(sizeof(NodePoint));
			::new(node)(NodePoint);
			alloc_count_++;
			return node;
		}
		void release(NodePoint *node)
		{
			if (node == nullptr)
				return;
			auto &pos = node->pos;
			//std::cout << "del node:" << node << " pos:" << pos.x_ << "-" << pos.y_ << "\n";
			alloc_count_--;
			node->~Node();
			alloc_.deallocate(node, sizeof(NodePoint));
		}

	private:
		std::vector<NodePoint *> open_list_;

		struct NodeHash {
			size_t operator() (NodePoint *n) const {
				return n->pos.Hash();
			}
		};
		struct NodeEqual {
			bool operator()(NodePoint *a, NodePoint *b) const {
				return a->pos.IsSame(b->pos);
			}
		};
		std::unordered_set<NodePoint *, NodeHash, NodeEqual> close_list_;
		std::vector<NodePoint *> success_list_;

		//分配器
		Alloc alloc_;
		int alloc_count_ = 0;;

		//节点
		NodePoint *start_pos_ = nullptr;
		NodePoint *end_pos_ = nullptr;

		//步数
		int step_ = 0;
		State state_ = SEARCH_STATE_NON;
	};

struct AStarPointBase
{
	virtual int GetCost(const AStarPointBase &pos) = 0;
	virtual int DistanceMetric(const AStarPointBase &end) const = 0;
	virtual size_t Hash() = 0;
	virtual int IsSame(const AStarPointBase &tar) const = 0;
	virtual bool GetSuccessPos(std::vector<AStarPointBase> &sucessList, AStarPointBase *pos) = 0;
};

}
}